In [1]:
# Imports
import numpy as np
from pprint import pprint
from timeit import timeit
import this
In [1]:
""" Get a simple Markov Chain transition matrix working and print out.
"""
# State info
currentState = 0
stateDictionary = {0 : 'stand still',
1 : 'wave left arm',
2 : 'wave right arm'
}
# Numpy matrix
transitionMatrix = np.array([[0.33, 0.33, 0.34],
[0.2, 0.4, 0.4],
[0.05, 0.05, 0.9]]
)
for i in range(0, 5):
# Generate random number
randomNum = np.random.random()
# Accumulate the relevant line of the transition matrix
# Using numpy:
# currentStateTransitionProbabilities = np.cumsum(transitionMatrix[currentState])
# Using list comprehension:
currentStateTransitionProbabilities = [(sum(transitionMatrix[currentState][:i])) for i in range(1, len(transitionMatrix[currentState]) + 1)]
# Find the new current state by comparing randomNum to the accululated matrix
# todo: there must be a cleaner way to do this comparison, a is my original attempt
a = len([i for i in range(0, len(currentStateTransitionProbabilities)) if randomNum >= currentStateTransitionProbabilities[i]])
print "a: ", a
# Rewrite using enumerate
b = len([index for index, probability in enumerate(currentStateTransitionProbabilities) if probability <= randomNum])
print "b: ", b
# From http://www.petercollingridge.co.uk/book/export/html/362
c = sum([(0, 1)[probability <= randomNum] for probability in currentStateTransitionProbabilities])
print "c: ", c
# From https://stackoverflow.com/questions/9572833/break-list-comprehension
def end_of_loop():
raise StopIteration
# Breaking out a list comprehension, not sure how different from using if, from https://stackoverflow.com/questions/9572833/break-list-comprehension
d = list(end_of_loop() if probability >= randomNum else index for index, probability in enumerate(currentStateTransitionProbabilities))
print "d: ", d
# Readable code!
for index, probability in enumerate(currentStateTransitionProbabilities):
if probability <= randomNum:
e = index + 1
else:
break
print "e: ", e
f = [index + 1 for index, probability in enumerate(currentStateTransitionProbabilities) if probability <= randomNum][-1:]
print "f: ", f
# Choose a method.
currentState = a
print "randomNum: ", randomNum
print "currentStateTransitionProbabilities: ", currentStateTransitionProbabilities
print "So the new current state is: ", currentState
print "Do: ", stateDictionary[currentState]
print
In [45]:
#markov laughing, from Jonny
#markov p martix all rows sum to 1
decision = np.array([[0.2,0.3,0.5],
[0.3,0.4,0.3],
[0.8,0.1,0.1]])
#laugh states
event = ["hee","haa","hoo"]
#number of laughs
for i in range(3):
lastMove = 0
#length of laugh
for j in range(3):
#make a weighed random choice
lastMove = np.random.choice(3, p=decision[lastMove])
print lastMove,
print event[lastMove],
print "\n"
In [8]:
""" Get a simple Markov Chain transition matrix working and print out.
Using pure numpy, no cumulative matrix, idea from Jonny,
"""
# State info
currentState = 0
stateDictionary = {0 : 'stand still',
1 : 'wave left arm',
2 : 'wave right arm'
}
# Numpy matrix
transitionMatrix = np.array([[0.33, 0.33, 0.34],
[0.2, 0.4, 0.4],
[0.1, 0.1, 0.8]])
testLoops = 10
numberElementsPerAction = 2
arraySize = len(transitionMatrix[currentState])
lastState = 0
# Here run a number of test loops.
# In robot this will be a global (robot) state change ie
# the transition matrix will be changed after an 'action' is
# complete e.g. going from robot waiting to be tickled state
# to robot being tickled state.
for i in range(testLoops):
for j in range(numberElementsPerAction):
lastState = np.random.choice(arraySize, p = transitionMatrix[lastState])
print "lastState: %s " % lastState,
print "Do: ", stateDictionary[lastState],
print "\n"
In [72]:
# A simple choice function from Jonny
# State info
currentState = 0
stateDictionary = {0 : 'stand still',
1 : 'wave left arm',
2 : 'wave right arm'
}
wordDictionary = {0 : 'hee',
1 : "ha",
2 : "ho",
3 : "who",
4 : "tee",
5 : "ho ho",
6 : "so, tickly!",
7 : "no!",
8 : "not there",
9 : "sigh!"
}
# Numpy matrix
transitionMatrix = np.array([[0.33, 0.33, 0.34],
[0.2, 0.4, 0.4],
[0.05, 0.05, 0.9]]
)
transitionMatrixInvalid = np.array([[0.33, 0.33, 0.5],
[0.2, 0.4, 0.5],
[0.05, 0.05, 0.5]]
)
transitionMatrixWord = np.array([[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]]
)
def choice(inArray):
""" Simple function to implement a Markov transition function.
"""
randNum = np.random.random()
cum = 0
#print "Type array: {}".format(type(inArray))
#print "Type sum array: {}".format(type(np.sum(inArray)))
#print "sum, outside: {} ".format(sum(inArray))
# round() is necessary in the comparison for arrays with 'more' values
if round(np.sum(inArray)) != 1.0:
# print "sum, inside: ", sum(inArray)
print "not a P array"
# Will need to raise or handle exception in robot code.
else:
for count,i in enumerate(inArray):
cum +=i
if cum > randNum :
return count
# Loop a few times to test.
print "Test1:"
for i in range(5):
currentState = choice(transitionMatrix[currentState])
print "Current state: {} so do: {}".format(currentState, stateDictionary[currentState])
print "\n"
# And test with invalid array, debug.
#for i in range(5):
# currentState = choice(transitionMatrixInvalid[currentState])
# print "Current state: %s so do: %s" % (currentState, stateDictionary[currentState])
# And test with array from robot code, debug.
print "Test2:"
for i in range(5):
currentState = choice(transitionMatrixWord[currentState])
print "Current state: {} so do: {}".format(currentState, wordDictionary[currentState])
In [95]:
# A simple choice function from Jonny - copy for understanding weird comparison problem
# in summed array row.
# numpy produces <type 'numpy.float64'> and the 1.0 for comparison is <type 'float'>
# Basically don't compare floats for equality!
# State info
currentState = 0
stateDictionary = {0 : 'stand still',
1 : 'wave left arm',
2 : 'wave right arm'
}
wordDictionary = {0 : 'hee',
1 : "ha",
2 : "ho",
3 : "who",
4 : "tee",
5 : "ho ho",
6 : "so, tickly!",
7 : "no!",
8 : "not there",
9 : "sigh!"
}
# Numpy matrix
transitionMatrix = np.array([[0.33, 0.33, 0.34],
[0.2, 0.4, 0.4],
[0.05, 0.05, 0.9]]
)
transitionMatrixInvalid = np.array([[0.33, 0.33, 0.5],
[0.2, 0.4, 0.5],
[0.05, 0.05, 0.5]]
)
transitionMatrixWord = np.array([[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
[0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]]
)
def choice1(inArray):
""" Simple function to implement a Markov transition function.
"""
randNum = np.random.random()
cum = 0
print "sum, outside: ", np.sum(inArray)
if round(np.sum(inArray)) != 1:
print "sum, inside: ", np.sum(inArray)
print "not a P array"
else:
for count,i in enumerate(inArray):
cum +=i
if cum > randNum :
return count
def choice2(inArray):
""" Simple function to implement a Markov transition function.
"""
randNum = np.random.random()
cum = 0
print "sum, outside: ", np.sum(inArray)
if np.sum(inArray) != 1:
print "sum, inside: ", np.sum(inArray)
print "not a P array"
else:
for count,i in enumerate(inArray):
cum +=i
if cum > randNum :
return count
# What types?
sum = np.sum(transitionMatrix[0])
print "sum value: {} type: {} ".format(sum, type(sum))
print "1.0 type: ", type(1.0)
print "\n"
# Calling the choice funtion using round as part of the comparison, works.
# Test with small array, debug.
print "Test1:"
currentState = 0
for i in range(5):
currentState = choice1(transitionMatrix[currentState])
print "Current state: {} so do: {}".format(currentState, stateDictionary[currentState])
print "\n"
# And test with large array from robot code, debug.
print "Test2:"
currentState = 0
for i in range(5):
currentState = choice1(transitionMatrixWord[currentState])
print "Current state: {} so do: {}".format(currentState, wordDictionary[currentState])
print "\n"
# Calling the choice funtion without round as part of the comparison, weird results.
# Test with small array, debug.
print "Test3:"
currentState = 0
for i in range(5):
currentState = choice2(transitionMatrix[currentState])
print "Current state: {} so do: {}".format(currentState, stateDictionary[currentState])
print "\n"
# And test with large array from robot code, debug.
print "Test4:"
currentState = 0
for i in range(5):
currentState = choice2(transitionMatrixWord[currentState])
print "Current state: {} so do: {}".format(currentState, wordDictionary[currentState])
print "\n"
In [96]:
print abs(0.9999)
print abs(-1.0001)
In [10]:
%%file choice.py
# -*- coding: ascii -*-
""" Exploring unit tests.
Simple code to implement a simple Markov transition function.
"""
__author__ = "Mike McFarlane (mike@mikemcfarlane.co.uk)"
__version__ = "$Revision: 0 $"
__date__ = "$Date: 11-04-14"
__copyright__ = "Copyright (c) Mike McFarlane 2014"
__license__ = "TBC"
import random
import numpy as np
def choice2(inArray):
""" Simple function to implement a Markov transition function.
"""
randNum = np.random.random()
cum = 0
sumVal = np.sum(inArray)
if not abs(sumVal - 1.0) < 1e-10:
#print "not a P array"
raise ce.MatrixError("Not a valid array")
else:
for count, i in enumerate(inArray):
cum += i
if cum >= randNum:
return count
def main():
for i in range(5):
print "Choice{}: {}".format(i, choice2([0.25, 0.25, 0.25, 0.25]))
if __name__ == '__main__':
main()
In [13]:
!python choice.py
In [ ]:
# Chose from these dictionaries, then parse into ALAnimatedSpeech
self.actionSittingDictionary = {0 : 'animations/Sit/BodyTalk/BodyTalk_1',
1 : 'animations/Sit/BodyTalk/BodyTalk_10',
2 : 'animations/Sit/BodyTalk/BodyTalk_11',
3 : 'animations/Sit/BodyTalk/BodyTalk_12'
}
self.actionStandingDictionary = {0 : 'animations/Stand/Gestures/Enthusiastic_4',
1 : 'animations/Stand/Gestures/Enthusiastic_5',
2 : 'animations/Stand/Gestures/Hey_1',
3 : 'animations/Stand/Gestures/Hey_6'
}
if self.defaultPose == 'Sit':
self.actionDictionary = self.actionSittingDictionary
else:
self.actionDictionary = self.actionStandingDictionary
In [16]:
%%file python_motion_data_export_from_Choregraph_inc_run_code.py
# -*- coding: ascii -*-
""" Motion data for Markov_tickles.py
"""
__author__ = "Mike McFarlane mike@mikemcfarlane.co.uk"
__version__ = "Revision: 0.14"
__date__ = "Date: 15-04-14"
__copyright__ = "Copyright (c)Mike McFarlane 2014"
__license__ = "TBC"
##############################################################
# Move left arm left right.
leftArmMovementList0 = [
["LElbowRoll",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[-0.335904, -0.308292, -0.269942, -0.283748, -0.19631, -0.1733, -0.079726]],
["LElbowYaw",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[-1.19963, -1.20116, -1.18736, -1.08305, -1.12293, -1.11219, -1.11833]],
["LHand",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[0.2952, 0.2952, 0.2952, 0.2952, 0.2952, 0.2952, 0.2952]],
["LShoulderPitch",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[0.892746, 0.87127, 1.00933, 1.01393, 1.02927, 1.04615, 1.14125]],
["LShoulderRoll",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[0.110406, 0.478566, -0.0629361, -0.30991, 0.082794, 0.41107, -0.0798099]],
["LWristYaw",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[-0.377406, -1.30241, 0.191708, 0.960242, -0.599836, 0.582878, -0.314512]],
["RElbowRoll",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[0.40962, 0.40962, 0.40962, 0.40962, 0.40962, 0.40962, 0.40962]],
["RElbowYaw",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[1.20568, 1.20568, 1.20568, 1.20568, 1.20568, 1.20568, 1.20568]],
["RHand",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[0.2936, 0.2936, 0.2936, 0.2936, 0.2936, 0.2936, 0.2936]],
["RShoulderPitch",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[1.45888, 1.45888, 1.45888, 1.45888, 1.45888, 1.45888, 1.45888]],
["RShoulderRoll",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[-0.16418, -0.16418, -0.16418, -0.16418, -0.16418, -0.16418, -0.16418]],
["RWristYaw",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[0.0643861, 0.0643861, 0.0643861, 0.0628521, 0.0643861, 0.0643861, 0.0628521]]
]
##############################################################
# Move left arm forward and back.
leftArmMovementList1 = [
["LElbowRoll",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[-0.194776, -0.0873961, -0.237728, -0.049046, -0.0628521, -0.0352399, -0.222388, -0.10427]],
["LElbowYaw",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[-1.11986, -1.11833, -1.12293, -1.11219, -1.12293, -1.12293, -1.11986, -1.11986]],
["LHand",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[0.2948, 0.2948, 0.2944, 0.2944, 0.2948, 0.2948, 0.2944, 0.2944]],
["LShoulderPitch",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[0.753152, 1.01393, 0.605888, 1.13052, 0.705598, 1.17193, 0.766958, 1.19648]],
["LShoulderRoll",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[0.0352399, 0.030638, 0.0413761, 0.00149202, 0.032172, 0.024502, 0.0597839, 0.049046]],
["LWristYaw",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[-0.483252, -0.452572, -0.48632, -0.446436, -0.538476, -0.504728, -0.513932, -0.452572]],
["RElbowRoll",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[0.403484, 0.403484, 0.40195, 0.40195, 0.403484, 0.403484, 0.403484, 0.40195]],
["RElbowYaw",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[1.19801, 1.19801, 1.19801, 1.19801, 1.19801, 1.19801, 1.19801, 1.19801]],
["RHand",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[0.3056, 0.3056, 0.3056, 0.3056, 0.3056, 0.3056, 0.3056, 0.3056]],
["RShoulderPitch",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[1.46501, 1.46501, 1.46808, 1.46808, 1.46961, 1.46961, 1.47422, 1.47422]],
["RShoulderRoll",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[-0.182588, -0.182588, -0.0890141, -0.0859461, -0.0859461, -0.0859461, -0.0752079, -0.0752079]],
["RWristYaw",
[0.76, 1.12, 1.48, 1.84, 2.16, 2.52, 2.88, 3.24],
[0.102736, 0.102736, 0.102736, 0.102736, 0.102736, 0.102736, 0.102736, 0.102736]]
]
##############################################################
# Shake left arm in front, then open close hand.
leftArmMovementList2 = [
["LElbowRoll",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[-0.0613179, -0.076658, -0.0429101, -0.08126, -0.08126, -0.079726, -0.08126, -0.0429101]],
["LElbowYaw",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[-1.07998, -1.26713, -1.23338, -1.28707, -1.28707, -1.28707, -1.28707, -1.25025]],
["LHand",
[0.48, 0.92, 1.28, 1.64, 1.96, 2.2, 2.44, 2.68, 2.92, 3.12, 3.32],
[0.2948, 0.2948, 0.2944, 0.2948, 0, 0.00559998, 1, 0.9648, 0.29, 0.2904, 0.2904]],
["LShoulderPitch",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[1.21028, 1.20415, 1.21028, 1.1704, 1.1704, 1.17193, 1.17193, 1.20722]],
["LShoulderRoll",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[0.0229681, 0.00149202, -0.0107799, 0.0214341, 0.0214341, 0.0214341, 0.0214341, 0.029104]],
["LWristYaw",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[1.82387, -1.82387, 1.82387, -1.82387, -1.82387, -1.82387, -1.82387, -0.04146]],
["RElbowRoll",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[0.391212, 0.391212, 0.391212, 0.392746, 0.391212, 0.391212, 0.392746, 0.391212]],
["RElbowYaw",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[1.19801, 1.19801, 1.19801, 1.19801, 1.19801, 1.19801, 1.19801, 1.19801]],
["RHand",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[0.3056, 0.3056, 0.3056, 0.3056, 0.3056, 0.3056, 0.3056, 0.3056]],
["RShoulderPitch",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[1.48189, 1.48189, 1.48189, 1.48189, 1.48189, 1.48189, 1.48189, 1.48189]],
["RShoulderRoll",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[-0.070606, -0.070606, -0.070606, -0.070606, -0.070606, -0.070606, -0.070606, -0.070606]],
["RWristYaw",
[0.48, 0.92, 1.28, 1.64, 2.2, 2.68, 3.12, 3.32],
[0.102736, 0.102736, 0.102736, 0.102736, 0.102736, 0.102736, 0.102736, 0.102736]]
]
##############################################################
# Shake left hand behind back.
leftArmMovementList3 = [
["LElbowRoll",
[0.56, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[-0.0674541, -0.05058, -0.0444441, -0.0444441, -0.0613179, -0.0643861, -0.05825, -0.049046, -0.049046, -0.401866]],
["LElbowYaw",
[0.56, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[-1.18889, -1.18889, -1.17662, -1.17662, -1.16895, -1.18736, -1.17662, -1.18736, -1.18889, -1.18889]],
["LHand",
[0.56, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[0.2944, 0.2944, 0.2948, 0.2944, 0.2944, 0.2948, 0.2944, 0.2944, 0.2944, 0.2944]],
["LShoulderPitch",
[0.56, 0.96, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[2.07086, 2.06428, 2.05705, 2.05245, 2.07086, 2.04171, 2.06165, 2.01257, 2.01257, 1.50328]],
["LShoulderRoll",
[0.56, 0.72, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.44, 2.72, 3, 3.32],
[0.07359, 0.234641, 0.269336, -0.0844119, 0.167164, -0.122762, 0.174834, -0.185656, 0.10849, 0.168698, 0.168698, 0.11961]],
["LWristYaw",
[0.56, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[-0.0813439, -0.0767419, 0.291418, -0.760906, 0.431012, -0.688808, 0.466294, -0.319114, -0.319114, -0.205598]],
["RElbowRoll",
[0.56, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[0.400416, 0.400416, 0.400416, 0.400416, 0.400416, 0.400416, 0.400416, 0.400416, 0.400416, 0.400416]],
["RElbowYaw",
[0.56, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[1.20568, 1.20568, 1.20568, 1.20568, 1.20568, 1.20568, 1.20568, 1.20568, 1.20568, 1.20568]],
["RHand",
[0.56, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[0.2936, 0.2936, 0.2936, 0.2936, 0.2936, 0.2936, 0.2936, 0.2936, 0.2936, 0.2936]],
["RShoulderPitch",
[0.56, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[1.46961, 1.46961, 1.46961, 1.46808, 1.46808, 1.46961, 1.46808, 1.46961, 1.46961, 1.46808]],
["RShoulderRoll",
[0.56, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[-0.0859461, -0.0859461, -0.0844119, -0.0813439, -0.0752079, -0.070606, -0.070606, -0.069072, -0.069072, -0.067538]],
["RWristYaw",
[0.56, 0.92, 1.24, 1.48, 1.72, 1.96, 2.2, 2.72, 3, 3.32],
[0.0643861, 0.0643861, 0.0643861, 0.0643861, 0.0643861, 0.0643861, 0.0643861, 0.0643861, 0.0643861, 0.0643861]]
]
##############################################################
def main():
""" Simple code to test above motion data. """
# Choregraphe simplified export in Python.
from naoqi import ALProxy
from pprint import pprint
names = list()
times = list()
keys = list()
leftArmMovementList = [leftArmMovementList0, leftArmMovementList1, leftArmMovementList2, leftArmMovementList3]
choice = 2
for n, t, k in leftArmMovementList[1]:
names.append(n)
times.append(t)
keys.append(k)
try:
# uncomment the following line and modify the IP if you use this script outside Choregraphe.
IP = "mistcalf.local"
motion = ALProxy("ALMotion", IP, 9559)
posture = ALProxy("ALRobotPosture", IP, 9559)
motion.wakeUp()
posture.goToPosture("StandInit", 0.8)
# motion = ALProxy("ALMotion")
motion.angleInterpolation(names, keys, times, True)
posture.goToPosture("Crouch", 0.8)
motion.rest()
except BaseException, err:
print err
if __name__ == '__main__':
main()
In [17]:
!python python_motion_data_export_from_Choregraph_inc_run_code.py
In [18]:
# eg
# Setup 2 proxies.
try:
robotMotionProxy1 = ALProxy("ALMotion")
except Exception, e:
print "Could not create proxy to ALMotion. Error: ", e
try:
robotMotionProxy2 = ALProxy("ALMotion")
except Exception, e:
print "Could not create proxy to ALMotion. Error: ", e
# Give data to both proxies and call in background with 'post'.
robotMotionProxy1.post.angleInterpolation(namesLeft, keysLeft, timesLeft, True)
robotMotionProxy2.post.angleInterpolation(namesRight, keysRight, timesRight, True)
According to https://community.aldebaran-robotics.com/doc/1-22/dev/naoqi/index.html#naoqi-blocking-non-blocking
Non-blocking - calls By using the post object of a proxy, a task is created in a parallel thread. This enables you to do other work at the same time (e.g. walking while talking). Each post call generates a task id. You can use this task id to check if a task is running, or wait until the task is finished.
Blocking calls - Like normal method calls, simple calls are blocking - The next instruction will be executed after the end of the previous call. All calls can raise an exception and should be encapsulated in a try-catch block. Calls can have return values.
ALMotion.angleInterpolation() is a blocking call!
Therefore will need to combine motion data into a single list.
Motion can be made smoother or more interesting with:
In [48]:
leftArmMovementList0 = [
["LElbowRoll",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[-0.335904, -0.308292, -0.269942, -0.283748, -0.19631, -0.1733, -0.079726]],
["LElbowYaw",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[-1.19963, -1.20116, -1.18736, -1.08305, -1.12293, -1.11219, -1.11833]],
["LHand",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[0.2952, 0.2952, 0.2952, 0.2952, 0.2952, 0.2952, 0.2952]],
["LShoulderPitch",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[0.892746, 0.87127, 1.00933, 1.01393, 1.02927, 1.04615, 1.14125]],
["LShoulderRoll",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[0.110406, 0.478566, -0.0629361, -0.30991, 0.082794, 0.41107, -0.0798099]],
["LWristYaw",
[0.88, 1.28, 1.68, 2.04, 2.44, 2.84, 3.2],
[-0.377406, -1.30241, 0.191708, 0.960242, -0.599836, 0.582878, -0.314512]]
]
rightArmMovementList0 = [
["RElbowRoll",
[0.44, 0.56, 0.88, 1.28, 1.44, 1.8, 2.12],
[0.400416, 0.3636, 0.377406, 0.358998, 0.339056, 0.331386, 0.296104]],
["RElbowYaw",
[0.44, 0.56, 0.88, 1.28, 1.44, 1.8, 2.12],
[1.20568, 1.23023, 1.23023, 1.15199, 1.18114, 1.20875, 1.11978]],
["RHand",
[0.44, 0.56, 0.72, 0.88, 1.08, 1.28, 1.44, 1.6, 1.8, 1.96, 2.12],
[0.2928, 0.2928, 0.59, 0.5952, 0.86, 0.86, 0.5696, 0.23, 0.226, 0.3, 0.2976]],
["RShoulderPitch",
[0.44, 0.56, 0.88, 1.28, 1.44, 1.8, 2.12],
[1.45888, 1.42513, 1.28093, 1.21037, 1.16588, 1.41132, 1.41899]],
["RShoulderRoll",
[0.44, 0.56, 0.88, 1.28, 1.44, 1.8, 2.12],
[-0.122762, -0.0890141, -0.237812, -0.224006, -0.131966, -0.0874801, -0.115092]],
["RWristYaw",
[0.44, 0.56, 0.88, 1.28, 1.44, 1.8, 2.12],
[0.0689881, 1.15046, -0.567622, -1.74113, -0.564554, 0.822182, -0.170316]]
]
In [51]:
names = []
times = []
keys = []
movementList = []
movementList = leftArmMovementList0 + rightArmMovementList0
for n, t, k in movementList:
names.append(n)
times.append(t)
keys.append(k)
pprint (n)
pprint (t)
pprint (k)
print "\n"
pprint (names)
pprint (times)
pprint (keys)
print "\n"
RGB colour reference:
In [2]:
%%file tickle_eyes.py
# -*- coding: ascii -*-
""" Explore LED colour changes in a list.
"""
import sys
from naoqi import ALProxy
IP = "mistcalf.local"
PORT = 9559
try:
proxy = ALProxy("ALLeds", IP, PORT)
except Exception,e:
print "Could not create proxy to ALLeds"
print "Error was: ",e
sys.exit(1)
def convertRGBToHex(list):
if len(list) == 3:
colour = 256 * 256 * list[0] + 256 * list[1] + list[2]
else:
raise ValueError("Not a valid RGB list")
return colour
name = 'AllLeds'
duration = 1.0
numEyeChanges = 5
durationList = [duration] * numEyeChanges
RGBColourDict = { "red" : [255, 0, 0], # red
"green" : [0, 255, 0], # green
"blue" : [0, 0, 255], # blue
"yellow" : [255, 255, 0], # yellow
"pink" : [255, 51, 153], # pink
"purple" : [204, 0, 204], # purple
"orange" : [255, 153, 51] # orange
}
RGBList = [None] * 5
for i, j in enumerate(RGBList):
colour = RGBColourDict["yellow"] # Do Markov choice here.
RGBList[i] = convertRGBToHex(colour)
print RGBList
proxy.fadeListRGB(name, RGBList, durationList)
proxy.reset(name)
print "Done being a rainbow"
In [58]:
!python tickle_eyes.py
In [56]:
%%file list_reverse_timing.py
# -*- coding: ascii -*-
""" Time list reversal.
"""
from timeit import timeit
repeats = 10000000
print "reverse(): ", timeit('list.reverse()', 'list = [1, 2, 3]', number = repeats)
print
print "slice: ", timeit('newList = list[::-1]', 'list = [1, 2, 3]', number = repeats)
print
print "list comp:", timeit('newList = [i for i in reversed(list)]', 'list = [1, 2, 3]', number = repeats)
print
In [57]:
!python list_reverse_timing.py
In [41]:
oldList1 = [1, 2, 3]
newList1 = [i for i in reversed(oldList1)]
print newList1
In [42]:
oldList2 = [1, 2, 3]
newList2 = oldList2[::-1]
print newList2
In [64]:
oldList3 = [1, 2, 3]
newList3 = list(oldList3) # This should work!
newList3.reverse()
print newList3
How do I know what to do for any touch event?
How can I make TickleMeNAO into a game?
So?
Check which sensor was touched in tickled() (option 2) and then use tickled() as the central decision making function for the game. No definitive reasons for chosing this, seems like the most flexible option.
Problem statement:
Potential solutions:
Coroutines.
Some extra reading.
Moved to a code sprint to learn more.
In [12]:
%%file success.py
# -*- coding: ascii -*-
""" Explore some methods for showing success and failure.
"""
import sys
from naoqi import ALProxy
IP = "mistcalf.local"
PORT = 9559
WIN = True
try:
aupProxy = ALProxy("ALAudioPlayer", IP, PORT)
except Exception, e:
print "Could not creat proxy to ALAudioPlayer. Error: ", e
sys.exit(1)
try:
animatedSpeechProxy = ALProxy("ALAnimatedSpeech", IP, PORT)
except Exception, e:
print "Could not create proxy to ALAnimatedSpeech. Error: ", e
try:
motionProxy = ALProxy("ALMotion", IP, PORT)
except Exception, e:
print "Could not create proxy to ALMotion. Error: ", e
# set the local configuration
configuration = {"bodyLanguageMode":"contextual"}
# Wake the robot up.
motionProxy.wakeUp()
def gameWinPraise():
# animated speech with LEDs
# say the text with the local configuration
animatedSpeechProxy.say("You did that really well!", configuration)
def gameWinAnimation():
# animated speech with LEDs
animatedSpeechProxy.say("I am going to dance now!", configuration)
# play sound with animation
volume = 0.75
pan = 0.0
aupProxy.playFile("/home/nao/audio/mystic1.wav", volume, pan)
aupProxy.playFile("/home/nao/audio/heavyMetal1.wav", volume, pan)
aupProxy.playFile("/home/nao/audio/applause1.wav", volume, pan)
def gameLost():
# animated speech with LEDs
pass
def main():
if WIN:
gameWinPraise()
gameWinAnimation()
else:
gameLost()
if __name__ == "__main__":
main()
In [13]:
!python success.py
There are a few ways to maintain the robots position relative to the user:
In [8]:
%%file visual_panorama.py
# An example with visual panorama
import sys
from naoqi import ALProxy
import time
def main():
ip = "mistcalf.local"
try:
motion = ALProxy("ALMotion", ip, 9559)
panorama = ALProxy("ALPanoramaCompass", ip, 9559)
except:
print "Cannot create proxies: %s" % sys.exc_info()[0]
return
time.sleep(5)
motion.wakeUp()
rc = -1
rc = panorama.setupPanorama()
description = []
if (rc != 0):
print "Cannot load or setup panorama"
return
description = panorama.getCurrentPanoramaDescriptor()
print "Panorama " + str(description[0]) + " loaded and ready"
position = panorama.getCurrentPosition()
print "Angle is now " + str(position[0])
time.sleep(2)
print "Moving to position 0 with odometry"
motion.moveTo(0, 0, 3.14)
position = panorama.getCurrentPosition()
print "Angle is now " + str(position[0])
time.sleep(2)
print "Going to position 1.5"
position = panorama.goToPosition(1.5)
position = panorama.getCurrentPosition()
print "Angle is now " + str(position[0])
time.sleep(2)
motion.rest()
if __name__=="__main__":
main()
sys.exit(0)
In [10]:
!python visual_panorama.py
In [15]:
%%file basic_awareness.py
from naoqi import ALProxy, ALBroker, ALModule
import time
import sys
ip_robot = "mistcalf.local"
port_robot = 9559
# Global variable to store the humanEventWatcher module instance
humanEventWatcher = None
memory = None
class HumanTrackedEventWatcher(ALModule):
""" A module to react to HumanTracked and PeopleLeft events """
def __init__(self):
ALModule.__init__(self, "humanEventWatcher")
global memory
memory = ALProxy("ALMemory", ip_robot, port_robot)
memory.subscribeToEvent("ALBasicAwareness/HumanTracked",
"humanEventWatcher",
"onHumanTracked")
memory.subscribeToEvent("ALBasicAwareness/PeopleLeft",
"humanEventWatcher",
"onPeopleLeft")
self.speech_reco = ALProxy("ALSpeechRecognition", ip_robot, port_robot)
self.is_speech_reco_started = False
def onHumanTracked(self, key, value, msg):
""" callback for event HumanTracked """
print "got HumanTracked: detected person with ID:", str(value)
if value >= 0: # found a new person
self.start_speech_reco()
position_human = self.get_people_perception_data(value)
[x, y, z] = position_human
print "The tracked person with ID", value, "is at the position:", \
"x=", x, "/ y=", y, "/ z=", z
def onPeopleLeft(self, key, value, msg):
""" callback for event PeopleLeft """
print "got PeopleLeft: lost person", str(value)
self.stop_speech_reco()
def start_speech_reco(self):
""" start asr when someone's detected in event handler class """
if not self.is_speech_reco_started:
try:
self.speech_reco.setVocabulary(["yes", "no"], False)
except RuntimeError:
print "ASR already started"
self.speech_reco.setVisualExpression(True)
self.speech_reco.subscribe("BasicAwareness_Test")
self.is_speech_reco_started = True
print "start ASR"
def stop_speech_reco(self):
""" stop asr when someone's detected in event handler class """
if self.is_speech_reco_started:
self.speech_reco.unsubscribe("BasicAwareness_Test")
self.is_speech_reco_started = False
print "stop ASR"
def get_people_perception_data(self, id_person_tracked):
memory = ALProxy("ALMemory", ip_robot, port_robot)
memory_key = "PeoplePerception/Person/" + str(id_person_tracked) + \
"/PositionInWorldFrame"
return memory.getData(memory_key)
if __name__ == "__main__":
event_broker = ALBroker("event_broker", "0.0.0.0", 0,
ip_robot, port_robot)
global humanEventWatcher
humanEventWatcher = HumanTrackedEventWatcher()
basic_awareness = ALProxy("ALBasicAwareness", ip_robot, port_robot)
motion = ALProxy("ALMotion", ip_robot, port_robot)
#start
motion.wakeUp()
basic_awareness.setEngagementMode("FullyEngaged")
basic_awareness.startAwareness()
#loop on, wait for events until interruption
try:
while True:
time.sleep(1)
except KeyboardInterrupt:
print "Interrupted by user, shutting down"
#stop
basic_awareness.stopAwareness()
motion.rest()
event_broker.shutdown()
sys.exit(0)
In [17]:
!python basic_awareness.py
Visual panorama is too slow for use here. People perception is not reliable enough with face detection as dependent on lighting conditions. Visual compass is not great as the user will move within the reference frame. What to do????
There are some issues with Autonomous Life and my code whereby TickleMeNAO will just stop. Possibly something going wrong during subscribe/unsubscribe to events and callback.
To get behaviour working the behaviour flow in Choregraph will be:
Quick fixes to do now:
In [1]:
%%file stop_by_holding_both_hands.py
import time
import sys
from optparse import OptionParser
from naoqi import ALProxy
from naoqi import ALBroker
from naoqi import ALModule
NAO_IP = "mistcalf.local"
# Global variables to store module instances and proxies
HandTouch = None
touchProxy = None
memoryProxy = None
class HandTouchModule(ALModule):
""" Simple module for tickling NAO.
"""
def __init__(self, name):
""" Initialise module.
"""
ALModule.__init__(self, name)
# Globals for proxies
global touchProxy
global memoryProxy
self.rightHandFlag = False
self.leftHandFlag = False
self.subscriptionListRight = [
"HandRightBackTouched",
"HandRightLeftTouched",
"HandRightRightTouched"
]
self.subscriptionListLeft = [
"HandLeftBackTouched",
"HandLeftLeftTouched",
"HandLeftRightTouched"
]
# Setup proxies
try:
touchProxy = ALProxy("ALTouch")
except Exception, e:
print "Could not create proxy to ALTouch. Error: ", e
try:
memoryProxy = ALProxy("ALMemory")
except Exception, e:
print "Could not create proxy to ALTouch. Error: ", e
self.easySubscribeEventsRight()
self.easySubscribeEventsLeft()
def easySubscribeEventsRight(self):
for eventName in self.subscriptionListRight:
try:
memoryProxy.subscribeToEvent(eventName, self.getName(), "rightHandTouched")
#print "Subscribed to %s." % eventName
except Exception, e:
print "Subscribe exception error %s for %s." % (e, eventName)
def easySubscribeEventsLeft(self):
for eventName in self.subscriptionListLeft:
try:
memoryProxy.subscribeToEvent(eventName, self.getName(), "leftHandTouched")
#print "Subscribed to %s." % eventName
except Exception, e:
print "Subscribe exception error %s for %s." % (e, eventName)
def easyUnsubscribeEventsRight(self):
for eventName in self.subscriptionListRight:
try:
memoryProxy.unsubscribeToEvent(eventName, self.getName())
#print "Subscribed to %s." % eventName
except Exception, e:
print "Unsubscribe exception error %s for %s." % (e, eventName)
def easyUnsubscribeEventsLeft(self):
for eventName in self.subscriptionListLeft:
try:
memoryProxy.unsubscribeToEvent(eventName, self.getName())
#print "Subscribed to %s." % eventName
except Exception, e:
print "unsubscribe exception error %s for %s." % (e, eventName)
def rightHandTouched(self):
self.easyUnsubscribeEventsRight()
print "---------- right ----------"
self.rightHandFlag = True
print self.rightHandFlag
print self.leftHandFlag
if self.leftHandFlag:
print "stop"
time.sleep(1.0)
self.rightHandFlag = False
self.easySubscribeEventsRight()
def leftHandTouched(self):
self.easyUnsubscribeEventsLeft()
print "-------- left ---------"
self.leftHandFlag = True
print self.rightHandFlag
print self.leftHandFlag
if self.rightHandFlag:
print "stop"
time.sleep(1.0)
self.leftHandFlag = False
self.easySubscribeEventsLeft()
def main():
""" Main entry point
"""
parser = OptionParser()
parser.add_option("--pip",
help="Parent broker port. The IP address of your robot",
dest="pip")
parser.add_option("--pport",
help="Parent broker port. The port NAOqi is listening to",
dest="pport",
type="int")
parser.set_defaults(
pip=NAO_IP,
pport=9559)
(opts, args_) = parser.parse_args()
pip = opts.pip
pport = opts.pport
# We need this broker to be able to construct
# NAOqi modules and subscribe to other modules
# The broker must stay alive until the program exists
global myBroker
myBroker = ALBroker("myBroker",
"0.0.0.0", # listen to anyone
0, # find a free port and use it
pip, # parent broker IP
pport) # parent broker port
# Warning: Objects must be a global variable
# The name given to the constructor must be the name of the
# variable
global HandTouch
HandTouch = HandTouchModule("HandTouch")
print "Running, hit CTRL+C to stop script"
while True:
time.sleep(1)
try:
while True:
time.sleep(1)
except KeyboardInterrupt:
print "Interrupted by user, shutting down"
# stop any post tasks
# eg void ALModule::stop(const int& id)
try:
myBroker.shutdown()
except Exception, e:
print "Error shutting down broker: ", e
try:
sys.exit(0)
except Exception, e:
print "Error exiting system: ", e
if __name__ == "__main__":
main()
In [2]:
!python stop_by_holding_both_hands.py
In [ ]: